/* +------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)            |
|                          https://www.mrpt.org/                         |
|                                                                        |
| Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
| See: https://www.mrpt.org/Authors - All rights reserved.               |
| Released under BSD License. See: https://www.mrpt.org/License          |
+------------------------------------------------------------------------+ */

#include "maps-precomp.h"  // Precomp header
//
#include <mrpt/core/bits_mem.h>
#include <mrpt/img/color_maps.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/serialization/aligned_serialization.h>
#include <mrpt/system/os.h>

#include "CPointsMap_crtp_common.h"

using namespace std;
using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::img;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::config;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
    "mrpt::maps::CColouredPointsMap,colourPointsMap", mrpt::maps::CColouredPointsMap)

CColouredPointsMap::TMapDefinition::TMapDefinition() = default;
void CColouredPointsMap::TMapDefinition::loadFromConfigFile_map_specific(
    const CConfigFileBase& source, const std::string& sectionNamePrefix)
{
  insertionOpts.loadFromConfigFile(source, sectionNamePrefix + string("_insertOpts"));
  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix + string("_likelihoodOpts"));
  colourOpts.loadFromConfigFile(source, sectionNamePrefix + string("_colorOpts"));
}

void CColouredPointsMap::TMapDefinition::dumpToTextStream_map_specific(std::ostream& out) const
{
  this->insertionOpts.dumpToTextStream(out);
  this->likelihoodOpts.dumpToTextStream(out);
  this->colourOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap::Ptr CColouredPointsMap::internal_CreateFromMapDefinition(
    const mrpt::maps::TMetricMapInitializer& _def)
{
  const CColouredPointsMap::TMapDefinition& def =
      *dynamic_cast<const CColouredPointsMap::TMapDefinition*>(&_def);
  auto obj = CColouredPointsMap::Create();
  obj->insertionOptions = def.insertionOpts;
  obj->likelihoodOptions = def.likelihoodOpts;
  obj->colorScheme = def.colourOpts;
  return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CColouredPointsMap, CPointsMap, mrpt::maps)

void CColouredPointsMap::reserve(size_t newLength)
{
  m_x.reserve(newLength);
  m_y.reserve(newLength);
  m_z.reserve(newLength);
  m_color_R.reserve(newLength);
  m_color_G.reserve(newLength);
  m_color_B.reserve(newLength);
}

// Resizes all point buffers so they can hold the given number of points: newly
// created points are set to default values,
//  and old contents are not changed.
void CColouredPointsMap::resize(size_t newLength)
{
  m_x.resize(newLength, 0);
  m_y.resize(newLength, 0);
  m_z.resize(newLength, 0);
  m_color_R.resize(newLength, 1);
  m_color_G.resize(newLength, 1);
  m_color_B.resize(newLength, 1);
  mark_as_modified();
}

// Resizes all point buffers so they can hold the given number of points,
// *erasing* all previous contents
//  and leaving all points to default values.
void CColouredPointsMap::setSize(size_t newLength)
{
  m_x.assign(newLength, 0);
  m_y.assign(newLength, 0);
  m_z.assign(newLength, 0);
  m_color_R.assign(newLength, 1);
  m_color_G.assign(newLength, 1);
  m_color_B.assign(newLength, 1);
  mark_as_modified();
}

uint8_t CColouredPointsMap::serializeGetVersion() const { return 9; }
void CColouredPointsMap::serializeTo(mrpt::serialization::CArchive& out) const
{
  uint32_t n = m_x.size();

  // First, write the number of points:
  out << n;

  if (n > 0)
  {
    out.WriteBufferFixEndianness(&m_x[0], n);
    out.WriteBufferFixEndianness(&m_y[0], n);
    out.WriteBufferFixEndianness(&m_z[0], n);
  }
  out << m_color_R << m_color_G << m_color_B;  // added in v4

  out << genericMapParams;               // v9
  insertionOptions.writeToStream(out);   // version 9?: insert options are saved with its own method
  likelihoodOptions.writeToStream(out);  // Added in version 5
}

void CColouredPointsMap::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 8:
    case 9:
    {
      mark_as_modified();

      // Read the number of points:
      uint32_t n;
      in >> n;

      this->resize(n);

      if (n > 0)
      {
        in.ReadBufferFixEndianness(&m_x[0], n);
        in.ReadBufferFixEndianness(&m_y[0], n);
        in.ReadBufferFixEndianness(&m_z[0], n);
      }
      in >> m_color_R >> m_color_G >> m_color_B;

      if (version >= 9)
        in >> genericMapParams;
      else
      {
        bool disableSaveAs3DObject;
        in >> disableSaveAs3DObject;
        genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
      }
      insertionOptions.readFromStream(in);
      likelihoodOptions.readFromStream(in);
    }
    break;

    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    case 7:
    {
      mark_as_modified();

      // Read the number of points:
      uint32_t n;
      in >> n;

      this->resize(n);

      if (n > 0)
      {
        in.ReadBufferFixEndianness(&m_x[0], n);
        in.ReadBufferFixEndianness(&m_y[0], n);
        in.ReadBufferFixEndianness(&m_z[0], n);

        // Version 1: weights are also stored:
        // Version 4: Type becomes long int -> uint32_t for
        // portability!!
        if (version >= 1)
        {
          if (version >= 4)
          {
            if (version >= 7)
            {
              // Weights were removed from this class in v7 (MRPT
              // 0.9.5),
              //  so nothing else to do.
            }
            else
            {
              // Go on with old serialization format, but discard
              // weights:
              std::vector<uint32_t> dummy_pointWeight(n);
              in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
            }
          }
          else
          {
            std::vector<uint32_t> dummy_pointWeight(n);
            in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
          }
        }
      }

      if (version >= 2)
      {
        // version 2: options saved too
        in >> insertionOptions.minDistBetweenLaserPoints >>
            insertionOptions.addToExistingPointsMap >> insertionOptions.also_interpolate >>
            insertionOptions.disableDeletion >> insertionOptions.fuseWithExisting >>
            insertionOptions.isPlanarMap;

        if (version < 6)
        {
          bool old_matchStaticPointsOnly;
          in >> old_matchStaticPointsOnly;
        }

        in >> insertionOptions.maxDistForInterpolatePoints;
        {
          bool disableSaveAs3DObject;
          in >> disableSaveAs3DObject;
          genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
        }
      }

      if (version >= 3)
      {
        in >> insertionOptions.horizontalTolerance;
      }

      if (version >= 4)  // Color data
      {
        in >> m_color_R >> m_color_G >> m_color_B;
        if (version >= 7)
        {
          // Removed: in >> m_min_dist;
        }
        else
        {
          std::vector<float> dummy_dist;
          in >> dummy_dist;
        }
      }
      else
      {
        m_color_R.assign(m_x.size(), 1.0f);
        m_color_G.assign(m_x.size(), 1.0f);
        m_color_B.assign(m_x.size(), 1.0f);
        // m_min_dist.assign(x.size(),2000.0f);
      }

      if (version >= 5)  // version 5: added likelihoodOptions
        likelihoodOptions.readFromStream(in);

      if (version >= 8)  // version 8: added insertInvalidPoints
        in >> insertionOptions.insertInvalidPoints;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
Clear
---------------------------------------------------------------*/
void CColouredPointsMap::internal_clear()
{
  // This swap() thing is the only way to really deallocate the memory.
  vector_strong_clear(m_x);
  vector_strong_clear(m_y);
  vector_strong_clear(m_z);

  vector_strong_clear(m_color_R);
  vector_strong_clear(m_color_G);
  vector_strong_clear(m_color_B);

  mark_as_modified();
}

/** Changes a given point from map. First index is 0.
 * \exception Throws std::exception on index out of bound.
 */
void CColouredPointsMap::setPointRGB(
    size_t index, float x, float y, float z, float R, float G, float B)
{
  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
  m_x[index] = x;
  m_y[index] = y;
  m_z[index] = z;
  this->m_color_R[index] = R;
  this->m_color_G[index] = G;
  this->m_color_B[index] = B;
  mark_as_modified();
}

/** Changes just the color of a given point from the map. First index is 0.
 * \exception Throws std::exception on index out of bound.
 */
void CColouredPointsMap::setPointColor(size_t index, float R, float G, float B)
{
  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
  this->m_color_R[index] = R;
  this->m_color_G[index] = G;
  this->m_color_B[index] = B;
  // mark_as_modified();  // No need to rebuild KD-trees, etc...
}

void CColouredPointsMap::insertPointRGB(float x, float y, float z, float R, float G, float B)
{
  m_x.push_back(x);
  m_y.push_back(y);
  m_z.push_back(z);
  m_color_R.push_back(R);
  m_color_G.push_back(G);
  m_color_B.push_back(B);

  mark_as_modified();
}

void CColouredPointsMap::getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const
{
  if (!genericMapParams.enableSaveAs3DObject) return;

  opengl::CPointCloudColoured::Ptr obj = std::make_shared<opengl::CPointCloudColoured>();

  obj->loadFromPointsMap(this);
  obj->setColor(1, 1, 1, 1.0);

  obj->setPointSize(this->renderOptions.point_size);

  o.insert(obj);
}

/*---------------------------------------------------------------
TColourOptions
---------------------------------------------------------------*/
CColouredPointsMap::TColourOptions::TColourOptions()

    = default;

void CColouredPointsMap::TColourOptions::loadFromConfigFile(
    const CConfigFileBase& source, const std::string& section)
{
  scheme = source.read_enum(section, "scheme", scheme);
  MRPT_LOAD_CONFIG_VAR(z_min, float, source, section)
  MRPT_LOAD_CONFIG_VAR(z_max, float, source, section)
  MRPT_LOAD_CONFIG_VAR(d_max, float, source, section)
}

void CColouredPointsMap::TColourOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CColouredPointsMap::TColourOptions] ------------ "
         "\n\n";

  out << "scheme                                  = " << scheme << endl;
  out << "z_min                                   = " << z_min << endl;
  out << "z_max                                   = " << z_max << endl;
  out << "d_max                                   = " << d_max << endl;
}

void CColouredPointsMap::getPointRGB(
    size_t index, float& x, float& y, float& z, float& R, float& G, float& B) const
{
  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");

  x = m_x[index];
  y = m_y[index];
  z = m_z[index];
  R = m_color_R[index];
  G = m_color_G[index];
  B = m_color_B[index];
}

/** Retrieves a point color (colors range is [0,1])
 */
void CColouredPointsMap::getPointColor(size_t index, float& R, float& G, float& B) const
{
  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");

  R = m_color_R[index];
  G = m_color_G[index];
  B = m_color_B[index];
}

// This function is duplicated from
// "mrpt::vision::pinhole::projectPoint_with_distortion", to avoid
//  a dependency on mrpt-vision.
static void aux_projectPoint_with_distortion(
    const mrpt::math::TPoint3D& P,
    const TCamera& params,
    TPixelCoordf& pixel,
    [[maybe_unused]] bool accept_points_behind)
{
  // Pinhole model:
  const double x = P.x / P.z;
  const double y = P.y / P.z;

  // Radial distortion:
  const double r2 = square(x) + square(y);
  const double r4 = square(r2);
  const double r6 = r2 * r4;

  pixel.x =
      params.cx() +
      params.fx() * (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6) +
                     2 * params.dist[2] * x * y + params.dist[3] * (r2 + 2 * square(x)));
  pixel.y =
      params.cy() +
      params.fy() * (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6) +
                     2 * params.dist[3] * x * y + params.dist[2] * (r2 + 2 * square(y)));
}

/*---------------------------------------------------------------
getPoint
---------------------------------------------------------------*/
bool CColouredPointsMap::colourFromObservation(
    const CObservationImage& obs, const CPose3D& robotPose)
{
  // Check if image is not grayscale
  ASSERT_(obs.image.isColor());

  CPose3D cameraPoseR;  // Get Camera Pose on the Robot(B) (CPose3D)
  CPose3D cameraPoseW;  // World Camera Pose

  obs.getSensorPose(cameraPoseR);
  cameraPoseW = robotPose + cameraPoseR;  // Camera Global Coordinates

  // Image Information
  unsigned int imgW = obs.image.getWidth();
  unsigned int imgH = obs.image.getHeight();

  // Projection related variables
  std::vector<TPixelCoordf> projectedPoints;        // The set of projected points in the image
  std::vector<TPixelCoordf>::iterator itProPoints;  // Iterator for projectedPoints
  std::vector<size_t> p_idx;
  std::vector<float> p_dist;
  std::vector<unsigned int> p_proj;

  // Get the N closest points
  kdTreeNClosestPoint2DIdx(
      cameraPoseW.x(), cameraPoseW.y(),  // query point
      200000,                            // number of points to search
      p_idx, p_dist);                    // indexes and distances of the returned points

  // Fill p3D vector
  for (size_t k = 0; k < p_idx.size(); k++)
  {
    float d = sqrt(p_dist[k]);
    size_t idx = p_idx[k];
    if (d < colorScheme.d_max)  //  && d < m_min_dist[idx] )
    {
      TPixelCoordf px;
      aux_projectPoint_with_distortion(
          TPoint3D(m_x[idx], m_y[idx], m_z[idx]), obs.cameraParams, px, true);
      projectedPoints.push_back(px);
      p_proj.push_back(k);
    }  // end if
  }    // end for

  // Get channel order
  unsigned int chR, chG, chB;
  if (obs.image.getChannelsOrder()[0] == 'B')
  {
    chR = 2;
    chG = 1;
    chB = 0;
  }
  else
  {
    chR = 0;
    chG = 1;
    chB = 2;
  }

  unsigned int n_proj = 0;
  const float factor = 1.0f / 255;  // Normalize pixels:

  // Get the colour of the projected points
  size_t k;
  for (itProPoints = projectedPoints.begin(), k = 0; itProPoints != projectedPoints.end();
       ++itProPoints, ++k)
  {
    // Only get the points projected inside the image
    if (itProPoints->x >= 0 && itProPoints->x < imgW && itProPoints->y > 0 && itProPoints->y < imgH)
    {
      unsigned int ii = p_idx[p_proj[k]];
      uint8_t* p = obs.image((unsigned int)itProPoints->x, (unsigned int)itProPoints->y);

      m_color_R[ii] = p[chR] * factor;  // R
      m_color_G[ii] = p[chG] * factor;  // G
      m_color_B[ii] = p[chB] * factor;  // B
      // m_min_dist[ii]	= p_dist[p_proj[k]];

      n_proj++;
    }
  }  // end for

  return true;
}  // end colourFromObservation

void CColouredPointsMap::resetPointsMinDist([[maybe_unused]] float defValue)
{
  // m_min_dist.assign(x.size(),defValue);
}

bool CColouredPointsMap::save3D_and_colour_to_text_file(const std::string& file) const
{
  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;

  for (size_t i = 0; i < m_x.size(); i++)
    os::fprintf(
        f, "%f %f %f %d %d %d\n", m_x[i], m_y[i], m_z[i], (uint8_t)(255 * m_color_R[i]),
        (uint8_t)(255 * m_color_G[i]), (uint8_t)(255 * m_color_B[i]));

  os::fclose(f);
  return true;
}

// ================================ PLY files import & export virtual methods
// ================================

/** In a base class, reserve memory to prepare subsequent calls to
 * PLY_import_set_vertex */
void CColouredPointsMap::PLY_import_set_vertex_count(size_t N) { this->setSize(N); }

/** In a base class, will be called after PLY_import_set_vertex_count() once for
 * each loaded point.
 *  \param pt_color Will be nullptr if the loaded file does not provide color
 * info.
 */
void CColouredPointsMap::PLY_import_set_vertex(
    size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
{
  if (pt_color)
    this->setPointRGB(idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
  else
    this->setPoint(idx, pt.x, pt.y, pt.z);
}

/** In a base class, will be called after PLY_export_get_vertex_count() once for
 * each exported point.
 *  \param pt_color Will be nullptr if the loaded file does not provide color
 * info.
 */
void CColouredPointsMap::PLY_export_get_vertex(
    size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color, TColorf& pt_color) const
{
  pt_has_color = true;

  pt.x = m_x[idx];
  pt.y = m_y[idx];
  pt.z = m_z[idx];

  pt_color.R = m_color_R[idx];
  pt_color.G = m_color_G[idx];
  pt_color.B = m_color_B[idx];
}

namespace mrpt::maps::detail
{
using mrpt::maps::CColouredPointsMap;

template <>
struct pointmap_traits<CColouredPointsMap>
{
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called only once before inserting
   * points - this is the place to reserve memory in lric for extra working
   * variables. */
  inline static void internal_loadFromRangeScan2D_init(
      CColouredPointsMap& me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
    // Vars:
    //  [0] -> pR
    //  [1] -> pG
    //  [2] -> pB
    //  [3] -> Az_1_color
    lric.fVars.resize(4);

    ASSERT_NOT_EQUAL_(me.colorScheme.z_max, me.colorScheme.z_min);
    lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data */
  inline static void internal_loadFromRangeScan2D_prepareOneRange(
      CColouredPointsMap& me,
      [[maybe_unused]] const float gx,
      [[maybe_unused]] const float gy,
      const float gz,
      mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
    // Relative height of the point wrt the sensor:
    const float rel_z = gz - lric.HM(2, 3);  // m23;

    // Variable renaming:
    float& pR = lric.fVars[0];
    float& pG = lric.fVars[1];
    float& pB = lric.fVars[2];
    float Az_1_color = lric.fVars[3];

    // Compute color:
    switch (me.colorScheme.scheme)
    {
      // case cmFromHeightRelativeToSensor:
      case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
      case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
      {
        float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
        if (q < 0)
          q = 0;
        else if (q > 1)
          q = 1;

        if (me.colorScheme.scheme == CColouredPointsMap::cmFromHeightRelativeToSensorGray)
        {
          pR = pG = pB = q;
        }
        else
        {
          jet2rgb(q, pR, pG, pB);
        }
      }
      break;
      case CColouredPointsMap::cmFromIntensityImage:
      {
        // In 2D scans we don't have this channel.
        pR = pG = pB = 1.0;
      }
      break;
      default:
        THROW_EXCEPTION("Unknown color scheme");
    }
  }
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called after each
   * "{x,y,z}.push_back(...);" */
  inline static void internal_loadFromRangeScan2D_postPushBack(
      CColouredPointsMap& me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
    float& pR = lric.fVars[0];
    float& pG = lric.fVars[1];
    float& pB = lric.fVars[2];
    me.m_color_R.push_back(pR);
    me.m_color_G.push_back(pG);
    me.m_color_B.push_back(pB);
    // m_min_dist.push_back(1e4);
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called only once before inserting
   * points - this is the place to reserve memory in lric for extra working
   * variables. */
  inline static void internal_loadFromRangeScan3D_init(
      CColouredPointsMap& me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
    // Vars:
    //  [0] -> pR
    //  [1] -> pG
    //  [2] -> pB
    //  [3] -> Az_1_color
    //  [4] -> K_8u
    //  [5] -> cx
    //  [6] -> cy
    //  [7] -> fx
    //  [8] -> fy
    lric.fVars.resize(9);
    float& cx = lric.fVars[5];
    float& cy = lric.fVars[6];
    float& fx = lric.fVars[7];
    float& fy = lric.fVars[8];

    // unsigned int vars:
    //  [0] -> imgW
    //  [1] -> imgH
    //  [2] -> img_idx_x
    //  [3] -> img_idx_y
    lric.uVars.resize(4);
    unsigned int& imgW = lric.uVars[0];
    unsigned int& imgH = lric.uVars[1];
    unsigned int& img_idx_x = lric.uVars[2];
    unsigned int& img_idx_y = lric.uVars[3];

    // bool vars:
    //  [0] -> hasValidIntensityImage
    //  [1] -> hasColorIntensityImg
    //  [2] -> simple_3d_to_color_relation
    lric.bVars.resize(3);
    uint8_t& hasValidIntensityImage = lric.bVars[0];
    uint8_t& hasColorIntensityImg = lric.bVars[1];
    uint8_t& simple_3d_to_color_relation = lric.bVars[2];

    ASSERT_NOT_EQUAL_(me.colorScheme.z_max, me.colorScheme.z_min);
    lric.fVars[3] = 1.0 / (me.colorScheme.z_max - me.colorScheme.z_min);  // Az_1_color = ...
    lric.fVars[4] = 1.0f / 255;                                           // K_8u

    hasValidIntensityImage = false;
    imgW = 0;
    imgH = 0;

    if (lric.rangeScan.hasIntensityImage)
    {
      // assure the size matches:
      if (lric.rangeScan.points3D_x.size() ==
          lric.rangeScan.intensityImage.getWidth() * lric.rangeScan.intensityImage.getHeight())
      {
        hasValidIntensityImage = true;
        imgW = lric.rangeScan.intensityImage.getWidth();
        imgH = lric.rangeScan.intensityImage.getHeight();
      }
    }

    hasColorIntensityImg = hasValidIntensityImage && lric.rangeScan.intensityImage.isColor();

    // running indices for the image pixels for the gray levels:
    // If both range & intensity images coincide (e.g. SwissRanger), then we
    // can just
    // assign 3D points to image pixels one-to-one, but that's not the case
    // if
    //
    simple_3d_to_color_relation =
        (std::abs(lric.rangeScan.relativePoseIntensityWRTDepth.norm()) < 1e-5);
    img_idx_x = 0;
    img_idx_y = 0;

    // Will be used just if simple_3d_to_color_relation=false
    cx = lric.rangeScan.cameraParamsIntensity.cx();
    cy = lric.rangeScan.cameraParamsIntensity.cy();
    fx = lric.rangeScan.cameraParamsIntensity.fx();
    fy = lric.rangeScan.cameraParamsIntensity.fy();
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data */
  inline static void internal_loadFromRangeScan3D_prepareOneRange(
      CColouredPointsMap& me,
      [[maybe_unused]] const float gx,
      [[maybe_unused]] const float gy,
      const float gz,
      mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
    // Rename variables:
    float& pR = lric.fVars[0];
    float& pG = lric.fVars[1];
    float& pB = lric.fVars[2];
    float& Az_1_color = lric.fVars[3];
    float& K_8u = lric.fVars[4];
    float& cx = lric.fVars[5];
    float& cy = lric.fVars[6];
    float& fx = lric.fVars[7];
    float& fy = lric.fVars[8];

    unsigned int& imgW = lric.uVars[0];
    unsigned int& imgH = lric.uVars[1];
    unsigned int& img_idx_x = lric.uVars[2];
    unsigned int& img_idx_y = lric.uVars[3];

    const uint8_t& hasValidIntensityImage = lric.bVars[0];
    const uint8_t& hasColorIntensityImg = lric.bVars[1];
    const uint8_t& simple_3d_to_color_relation = lric.bVars[2];

    // Relative height of the point wrt the sensor:
    const float rel_z = gz - lric.HM(2, 3);  // m23;

    // Compute color:
    switch (me.colorScheme.scheme)
    {
      // case cmFromHeightRelativeToSensor:
      case CColouredPointsMap::cmFromHeightRelativeToSensorJet:
      case CColouredPointsMap::cmFromHeightRelativeToSensorGray:
      {
        float q = (rel_z - me.colorScheme.z_min) * Az_1_color;
        if (q < 0)
          q = 0;
        else if (q > 1)
          q = 1;

        if (me.colorScheme.scheme == CColouredPointsMap::cmFromHeightRelativeToSensorGray)
        {
          pR = pG = pB = q;
        }
        else
        {
          jet2rgb(q, pR, pG, pB);
        }
      }
      break;
      case CColouredPointsMap::cmFromIntensityImage:
      {
        // Do we have to project the 3D point into the image plane??
        bool hasValidColor = false;
        if (simple_3d_to_color_relation)
        {
          hasValidColor = true;
        }
        else
        {
          // Do projection:
          TPoint3D pt;  // pt_wrt_colorcam;
          lric.rangeScan.relativePoseIntensityWRTDepth.inverseComposePoint(
              lric.scan_x, lric.scan_y, lric.scan_z, pt.x, pt.y, pt.z);

          // Project to image plane:
          if (pt.z)
          {
            img_idx_x = cx + fx * pt.x / pt.z;
            img_idx_y = cy + fy * pt.y / pt.z;

            hasValidColor = img_idx_x < imgW &&  // img_idx_x>=0
                                                 // isn't needed for
                                                 // unsigned.
                            img_idx_y < imgH;
          }
        }

        const auto& ii = lric.rangeScan.intensityImage;
        if (hasValidColor && hasColorIntensityImg)
        {
          const auto* c = ii.ptr<uint8_t>(img_idx_x, img_idx_y);
          pR = c[2] * K_8u;
          pG = c[1] * K_8u;
          pB = c[0] * K_8u;
        }
        else if (hasValidColor && hasValidIntensityImage)
        {
          const auto c = ii.at<uint8_t>(img_idx_x, img_idx_y);
          pR = pG = pB = c * K_8u;
        }
        else
        {
          pR = pG = pB = 1.0;
        }
      }
      break;
      default:
        THROW_EXCEPTION("Unknown color scheme");
    }
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called after each
   * "{x,y,z}.push_back(...);" */
  inline static void internal_loadFromRangeScan3D_postPushBack(
      CColouredPointsMap& me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
    float& pR = lric.fVars[0];
    float& pG = lric.fVars[1];
    float& pB = lric.fVars[2];

    me.m_color_R.push_back(pR);
    me.m_color_G.push_back(pG);
    me.m_color_B.push_back(pB);

    // m_min_dist.push_back(1e4);
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
   * end */
  inline static void internal_loadFromRangeScan3D_postOneRange(
      [[maybe_unused]] CColouredPointsMap& me,
      mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
    unsigned int& imgW = lric.uVars[0];
    unsigned int& img_idx_x = lric.uVars[2];
    unsigned int& img_idx_y = lric.uVars[3];

    const uint8_t& hasValidIntensityImage = lric.bVars[0];
    const uint8_t& simple_3d_to_color_relation = lric.bVars[2];

    // Advance the color pointer (just for simple cases, e.g. SwissRanger,
    // not for Kinect)
    if (simple_3d_to_color_relation && hasValidIntensityImage)
    {
      if (++img_idx_x >= imgW)
      {
        img_idx_y++;
        img_idx_x = 0;
      }
    }
  }
};
}  // namespace mrpt::maps::detail
/** See CPointsMap::loadFromRangeScan() */
void CColouredPointsMap::loadFromRangeScan(
    const CObservation2DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  mrpt::maps::detail::loadFromRangeImpl<CColouredPointsMap>::templ_loadFromRangeScan(
      *this, rangeScan, robotPose);
}

/** See CPointsMap::loadFromRangeScan() */
void CColouredPointsMap::loadFromRangeScan(
    const CObservation3DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  mrpt::maps::detail::loadFromRangeImpl<CColouredPointsMap>::templ_loadFromRangeScan(
      *this, rangeScan, robotPose);
}
